#include "RandomParticleVehicleDataPredictor.h"

#include <mrpt/core.h>

using namespace mrpt::math;

CRandomParticleVehicleDataPredictor::CRandomParticleVehicleDataPredictor(const CParticleVehicleData& stdDev) {
	this->stdDev = stdDev;
}

void CRandomParticleVehicleDataPredictor::predict(CParticleVehicleData* particle) {

	particle->x = deltaTime * particle->vx + stdDev.x * mrpt::random::normalizedGaussian();
	particle->y = deltaTime * particle->vy + stdDev.y * mrpt::random::normalizedGaussian();
	particle->w = deltaTime * particle->vw + stdDev.w * mrpt::random::normalizedGaussian();

	particle->vx += stdDev.vx * mrpt::random::normalizedGaussian();
	particle->vy += stdDev.vy * mrpt::random::normalizedGaussian();
	particle->vw += stdDev.vw * mrpt::random::normalizedGaussian();

}
